#include "fccu_queue.h"
#include <stdlib.h>

/* MISRA C: Function to initialize a FCCU_QueueSet structure.
 * Parameters:
 *   set       - Pointer to FCCU_QueueSet structure to initialize.
 *   item_size - Size of each item in the queue.
 * Returns:
 *   0  - Success.
 *  -1  - Failure (e.g., set is NULL or memory allocation fails).
 */
int32_t Fccu_QueueSetInit(struct FCCU_QueueSet *set, uint32_t item_size)
{
    uint8_t i;
    if (set == NULL)
    {
        return -1;
    }
    set->level_count = FCCU_QUEUE_LEVELS;
    set->item_size = item_size;
    for (i = 0U; i < FCCU_QUEUE_LEVELS; ++i)
    {
        set->queue[i] = SpscQueue_Create(FCCU_QUEUE_LEVEL_SIZE * item_size);
        if (set->queue[i] == NULL)
        {
            uint8_t j;
            for (j = 0U; j < i; ++j)
            {
                SpscQueue_Destroy(set->queue[j]);
                set->queue[j] = NULL;
            }
            return -1;
        }
    }
    return 0;
}

/* MISRA C: Function to destroy a FCCU_QueueSet structure and free resources.
 * Parameters:
 *   set - Pointer to FCCU_QueueSet structure to destroy.
 * Returns:
 *   None.
 */
void Fccu_QueueSetDestroy(struct FCCU_QueueSet *set)
{
    uint8_t i;
    if (set == NULL)
    {
        return;
    }
    for (i = 0U; i < set->level_count; ++i)
    {
        if (set->queue[i] != NULL)
        {
            SpscQueue_Destroy(set->queue[i]);
            set->queue[i] = NULL;
        }
    }
}

/* MISRA C: Function to push an item into a specific level of the queue set.
 * Parameters:
 *   set   - Pointer to FCCU_QueueSet structure.
 *   level - Level index to push the item to.
 *   data  - Pointer to the data to be pushed.
 * Returns:
 *   0  - Failure (invalid parameters).
 *   >0 - Number of bytes pushed (success).
 */
int32_t Fccu_QueueSetPush(struct FCCU_QueueSet *set, uint8_t level, const void *data)
{
    if ((set == NULL) || (level >= set->level_count) || (set->queue[level] == NULL) || (data == NULL))
    {
        return 0;
    }
    return SpscQueue_Push(set->queue[level], (const uint8_t *)data, set->item_size);
}

/* MISRA C: Function to pop an item from the queue set, prioritizing lower levels.
 * Parameters:
 *   set       - Pointer to FCCU_QueueSet structure.
 *   data      - Pointer to buffer to store popped data.
 *   out_level - Pointer to store the level index from which data was popped (can be NULL).
 * Returns:
 *   1 - Success (item popped).
 *   0 - Failure (no item available or invalid parameters).
 */
int32_t Fccu_QueueSetPop(struct FCCU_QueueSet *set, void *data, uint8_t *out_level)
{
    uint8_t i;
    if ((set == NULL) || (data == NULL))
    {
        return 0;
    }
    for (i = 0U; i < set->level_count; ++i)
    {
        if (SpscQueue_DataLen(set->queue[i]) >= set->item_size)
        {
            uint32_t ret = SpscQueue_Pop(set->queue[i], (uint8_t *)data, set->item_size);
            if (ret == set->item_size)
            {
                if (out_level != NULL)
                {
                    *out_level = i;
                }
                return 1;
            }
        }
    }
    return 0;
}

/* MISRA C: Function to check if all queues in the set are empty.
 * Parameters:
 *   set - Pointer to FCCU_QueueSet structure.
 * Returns:
 *   true  - All queues are empty or set is NULL.
 *   false - At least one queue is not empty.
 */
bool Fccu_QueueSetIsEmpty(struct FCCU_QueueSet *set)
{
    uint8_t i;
    if (set == NULL)
    {
        return true;
    }
    for (i = 0U; i < set->level_count; ++i)
    {
        if (SpscQueue_DataLen(set->queue[i]) > 0U)
        {
            return false;
        }
    }
    return true;
}